
#include "Octree.h"
#include "Real2.h"
#include <cmath>

#define MAX_TREE_LEVEL 6
#define SMALLEST_SIDE_LENGTH 0.2f
#define SMALLEST_VOLUME 0.0005f

#define CheckAxis1(A,B,C) \
{ \
	float p0 = A.z*B.y - A.y*B.z; \
	float p1 = C.y*(A.z-B.z) + C.z*(B.y-A.y); \
	float r  = R.y*fabs(A.z-B.z) + R.z*fabs(B.y-A.y); \
	if(min(p0,p1) > r || max(p0,p1) < -r) return false; \
}
#define CheckAxis2(A,B,C) \
{ \
	float p0 = A.x*B.z - A.z*B.x; \
	float p1 = C.z*(A.x-B.x) + C.x*(B.x-A.x); \
	float r  = R.z*fabs(A.x-B.x) + R.x*fabs(B.x-A.x); \
	if(min(p0,p1) > r || max(p0,p1) < -r) return false; \
}
#define CheckAxis3(A,B,C) \
{ \
	float p0 = A.y*B.x - A.x*B.y; \
	float p1 = C.x*(A.y-B.y) + C.y*(B.x-A.x); \
	float r  = R.x*fabs(A.y-B.y) + R.y*fabs(B.x-A.x); \
	if(min(p0,p1) > r || max(p0,p1) < -r) return false; \
}

//
// Constructor
//

Octree::Octree(const Real3 &_point, const Real3 &_extent, uchar _level)
{
	box = BBox(_point, _extent);

	// No tree nodes yet
	for(uint i = 0; i < 8; ++i)
		oct[i] = 0;

	level = _level;

	// Determine if it is small enough
	if(level > MAX_TREE_LEVEL) 
		smallEnough = true;
	//else if(box.extent.x < SMALLEST_SIDE_LENGTH || box.extent.y < SMALLEST_SIDE_LENGTH || box.extent.z < SMALLEST_SIDE_LENGTH)
	else if(box.Volume() < SMALLEST_VOLUME)
		smallEnough = true;
	else smallEnough = false;

	isOccupied = false;
}

//
// Destructor
//

Octree::~Octree()
{
	if(isOccupied)
	{
		for(ushort i = 0; i < 8; ++i)
			delete oct[i];
	}
}

//
// TriCubeOverlap
//

#define USE_SEPARATING_AXIS_THEOREM_OPTIMIZED
//#define USE_SEPARATING_AXIS_THEOREM

#ifdef USE_SEPARATING_AXIS_THEOREM
const Real3 e[] =
{
	Real3(1.0f, 0.0f, 0.0f),
	Real3(0.0f, 1.0f, 0.0f),
	Real3(0.0f, 0.0f, 1.0f)
};
#endif

bool TriCubeOverlap(BBox box, Real3 a, Real3 b, Real3 c)
{
	// Move center to (0,0,0)
	Real3 dist = box.Centralize();

	// Adjust points
	a -= dist;
	b -= dist;
	c -= dist;

	// Create BBox from triangle
	BBox triBox(a, b, c);

	// Makes sure the BBoxes are overlapping (determines if they are 'close')
	if(!BBoxCollides(box, triBox)) return false;

	// Long drawn out checking (expanded for optimizations)

	// Box Radius (half extent)
	Real3 R = 0.5f * box.Extent();

#ifdef USE_SEPARATING_AXIS_THEOREM
	Real3 f[] = { b-a, c-b, a-c };

	for(ushort i = 0; i < 3; ++i)
	for(ushort j = 0; j < 3; ++j)
	{
		Real3 axis = Cross(e[i], f[j]);

		float p0 = Dot(axis, a);
		float p1 = Dot(axis, b);
		float p2 = Dot(axis, c);

		float r =  Dot(Abs(axis), R);

		if(min(min(p0,p1),p2) > r || max(max(p0,p1),p2) < -r) return false;
	}
#endif

#ifdef USE_SEPARATING_AXIS_THEOREM_OPTIMIZED
	CheckAxis1(a,b,c);
	CheckAxis2(a,b,c);
	CheckAxis3(a,b,c);

	CheckAxis1(b,c,a);
	CheckAxis2(b,c,a);
	CheckAxis3(b,c,a);
	
	CheckAxis1(c,a,b);
	CheckAxis2(c,a,b);
	CheckAxis3(c,a,b);
	/*// 0 0
	{
		float p0 = a.z*b.y - a.y*b.z;
		float p1 = c.y*(a.z-b.z) + c.z*(b.y-a.y);
		float r  = R.y*fabs(a.z-b.z) + R.z*fabs(b.y-a.y);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 0 1
	{
		float p0 = a.x*b.z - a.z*b.x;
		float p1 = c.z*(a.x-b.x) + c.x*(b.x-a.x);
		float r  = R.z*fabs(a.x-b.x) + R.x*fabs(b.x-a.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 0 1
	{
		float p0 = a.y*b.x - a.x*b.y;
		float p1 = c.x*(a.y-b.y) + c.y*(b.x-a.x);
		float r  = R.x*fabs(a.y-b.y) + R.y*fabs(b.x-a.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 1 0
	{
		float p0 = b.z*c.y - b.y*c.z;
		float p1 = a.y*(b.z-c.z) + a.z*(c.y-b.y);
		float r  = R.y*fabs(b.z-c.z) + R.z*fabs(c.y-b.y);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 1 1
	{
		float p0 = b.x*c.z - b.z*c.x;
		float p1 = a.z*(b.x-c.x) + a.x*(c.x-b.x);
		float r  = R.z*fabs(b.x-c.x) + R.x*fabs(c.x-b.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 1 1
	{
		float p0 = b.y*c.x - b.x*c.y;
		float p1 = a.x*(b.y-c.y) + a.y*(c.x-b.x);
		float r  = R.x*fabs(b.y-c.y) + R.y*fabs(c.x-b.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 2 0
	{
		float p0 = c.z*a.y - c.y*a.z;
		float p1 = b.y*(c.z-a.z) + b.z*(a.y-c.y);
		float r  = R.y*fabs(c.z-a.z) + R.z*fabs(a.y-c.y);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 2 1
	{
		float p0 = c.x*a.z - c.z*a.x;
		float p1 = b.z*(c.x-a.x) + b.x*(a.x-c.x);
		float r  = R.z*fabs(c.x-a.x) + R.x*fabs(a.x-c.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}

	// 2 1
	{
		float p0 = c.y*a.x - c.x*a.y;
		float p1 = b.x*(c.y-a.y) + b.y*(a.x-c.x);
		float r  = R.x*fabs(c.y-a.y) + R.y*fabs(a.x-c.x);
		if(min(p0,p1) > r || max(p0,p1) < -r) return false;
	}*/
#endif

	return true;
}

//
// Add Tri
//

void Octree::AddTri(const Real3 &a, const Real3 &b, const Real3 &c)
{
	Occupy();

	if(!smallEnough)
	{
		for(ushort i = 0; i < 8; ++i)
			if(TriCubeOverlap(oct[i]->box, a, b, c))
				oct[i]->AddTri(a, b, c);
	}
}

//
// Add Box
//

void Octree::AddBox(const BBox &b)
{
	Occupy();

	if(!smallEnough)
	{
		for(ushort i = 0; i < 8; ++i)
			if(BBoxCollides(oct[i]->box, b))
				oct[i]->AddBox(b);
	}
}

//
// AddOctree
//

void Octree::AddOctree(const Octree &t)
{
	Occupy();

	if(!smallEnough)
	{
	}
}

#define ONLY_MIN_BOXES

//
// GetBoxPoints
//

void Octree::GetBoxPoints(vector<Real3> &ptr, const Real3 &pos)
{
#ifdef ONLY_MIN_BOXES
	if(smallEnough && isOccupied)
		box.CalculateVertexPoints(ptr, pos);
	else
#else
	box.CalculateVertexPoints(ptr, pos);
	if(!smallEnough)
#endif
		for(int i = 0; i < 8; ++i)
			if(oct[i]) oct[i]->GetBoxPoints(ptr, pos);
}

//
// Occupy
//

void Octree::Occupy()
{
	if(!isOccupied)
	{
		isOccupied = true;

		if(!smallEnough)
		{
			Real3 halfExt = 0.5f * box.extent;

			tree.rtb = new Octree(box.point + halfExt,                                halfExt, level+1);
			tree.rtf = new Octree(box.point + Real3(halfExt.x, halfExt.y,      0.0f), halfExt, level+1);
			tree.rbb = new Octree(box.point + Real3(halfExt.x,      0.0f, halfExt.z), halfExt, level+1);
			tree.rbf = new Octree(box.point + Real3(halfExt.x,      0.0f,      0.0f), halfExt, level+1);
			tree.ltb = new Octree(box.point + Real3(     0.0f, halfExt.y, halfExt.z), halfExt, level+1);
			tree.ltf = new Octree(box.point + Real3(     0.0f, halfExt.y,      0.0f), halfExt, level+1);
			tree.lbb = new Octree(box.point + Real3(     0.0f,      0.0f, halfExt.z), halfExt, level+1);
			tree.lbf = new Octree(box.point,                                          halfExt, level+1);
		}
	}
}

//
// OctreeCollisions
//

bool OctreeCollisions(const Octree *ocA, const Real3 &posA, const Octree *ocB, const Real3 &posB)
{
	if(ocA->smallEnough || ocB->smallEnough)
		return BBoxCollides(ocA->box + posA, ocB->box + posB);
	
	if(!BBoxCollides(ocA->box + posA, ocB->box + posB))
		return false;
	
	for(int i = 0; i < 8; ++i)
		if(ocA->oct[i] && ocA->oct[i]->isOccupied)
			for(int j = 0; j < 8; ++j)
				if(ocB->oct[j] && ocB->oct[j]->isOccupied && OctreeCollisions(ocA->oct[i], posA, ocB->oct[j], posB))
					return true;
	
	return false;
}


bool OctreeCollisions(const Octree *ocA, const D3DXMATRIX *aTrans, const Octree *ocB, const D3DXMATRIX *bTrans)
{
	if(ocA->smallEnough || ocB->smallEnough)
		return BBoxCollides(ocA->box, aTrans, ocB->box, bTrans);
	
	if(!BBoxCollides(ocA->box, aTrans, ocB->box, bTrans))
		return false;
	
	for(int i = 0; i < 8; ++i)
		if(ocA->oct[i] && ocA->oct[i]->isOccupied)
			for(int j = 0; j < 8; ++j)
				if(ocB->oct[j] && ocB->oct[j]->isOccupied && OctreeCollisions(ocA->oct[i], aTrans, ocB->oct[j], bTrans))
					return true;
	
	return false;
}